#include <QDebug>

#include <QFileDialog>
#include <QMessageBox>

#include "pclCloud.h"

pclCloud::pclCloud()
{
	m_cloud.reset(new PointCloudT);
	enable();
}

pclCloud::~pclCloud()
{
}

PointCloudT::Ptr pclCloud::loadFile(QString fileName)
{
	QFile file(fileName);
	if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
	{
		qDebug() << "Can't open the file!" << endl;
	}

	m_cloud->points.clear();
	m_cloud->width = 0;

	while (!file.atEnd())
	{
		QStringList strList;
		double x, y, z;
		QByteArray line = file.readLine();
		QString str(line);
		//qDebug()<< str;

		strList = str.split("\t");
		if (strList.size() < 3)
		{
			strList = str.split(" ");
		}

		QString num(strList[0]);
		x = num.toDouble();
		num = (strList[1]);
		y = num.toDouble();
		num = (strList[2]);
		z = num.toDouble();

		pcl::PointXYZRGBA pointXyzRgb;
		pointXyzRgb.x = x;
		pointXyzRgb.y = y;
		pointXyzRgb.z = z;

		m_cloud->push_back(pointXyzRgb);
		m_cloud->width++;
	}

	return m_cloud;
}

void pclCloud::saveFile(QString fileName)
{
	QFile file(fileName);
	if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
	{
		qDebug() << "Can't open the file!" << endl;
	}

	QTextStream out(&file);
	for (int i = 0; i < m_cloud->points.size(); i++)
	{
		out << (m_cloud->points[i].x) << " ";
		out << (m_cloud->points[i].y) << " ";
		out << (m_cloud->points[i].z) << " ";
		out << endl;
	}

	file.close();
}

void pclCloud::rotate(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr dat, float angle)
{
	int idx, size;
	double x, y, z;

	size = dat->points.size();
	for (idx = 0; idx < size; idx++)
	{
		x = dat->points[idx].x;
		y = dat->points[idx].y;
		z = dat->points[idx].z;

		dat->points[idx].x = x;
		dat->points[idx].y = y * cos(angle) - z * sin(angle);
		dat->points[idx].z = y * sin(angle) + z * cos(angle);
	}
	dat->width = idx;
}

void pclCloud::rotate(
	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr src,
	pcl::PointCloud<pcl::PointXYZRGBA>::Ptr dst,
	float angle)
{
	int idx, size;
	double x, y, z;

	size = src->points.size();

	dst->points.clear();
	for (idx = 0; idx < size; idx++)
	{
		x = src->points[idx].x;
		y = src->points[idx].y;
		z = src->points[idx].z;

		pcl::PointXYZRGBA pointXyzRgb;
		pointXyzRgb.x = x;
		pointXyzRgb.y = y * cos(angle) - z * sin(angle);
		pointXyzRgb.z = y * sin(angle) + z * cos(angle);

		pointXyzRgb.x = src->points[idx].r;
		pointXyzRgb.y = src->points[idx].g;
		pointXyzRgb.z = src->points[idx].b;

		dst->points.push_back(pointXyzRgb);
	}
	dst->width = size;
	src->width = size;
}

int pclCloud::normalize(PointCloudT::Ptr cloud) //txt2PcdNormalize4Color
{
	int n = 1;
	double maxValue = 0;
	double maxValueZ = 0;
	double minValueZ = 0;
	double maxValueX = 0;
	double minValueX = 0;
	double maxValueY = 0;
	double minValueY = 0;
	double maxValueTmp;
	double ratio;
	double deltaRgbX, deltaRgbY, deltaRgbZ = 0;
	double tmpX, tmpY, tmpZ;
	int width;

	width = cloud->width;

	double x, y, z;
	int i;
	uint8_t r(255), g(0), b(15);
	for (i = 0; i < width; i++)
	{
		//cout<<x<<" "<<y<<" "<<z<<endl;  
		x = cloud->points[i].x;
		y = cloud->points[i].y;
		z = cloud->points[i].z;

		maxValueTmp = myMax(x, y, z);
		if (maxValue < maxValueTmp)
		{
			maxValue = maxValueTmp;
		}

		if (maxValueZ < z)
		{
			maxValueZ = z;
		}
		if (minValueZ > z)
		{
			minValueZ = z;
		}

		if (maxValueX < x)
		{
			maxValueX = x;
		}
		if (minValueX > x)
		{
			minValueX = x;
		}

		if (maxValueY < y)
		{
			maxValueY = y;
		}
		if (minValueY > y)
		{
			minValueY = y;
		}
	}

	tmpX = maxValueX - minValueX;
	tmpY = maxValueY - minValueY;
	tmpZ = maxValueZ - minValueZ;

	//ratio = 1 / maxValue * 0.25;
	ratio = 1 / maxValue;
	deltaRgbX = 255 / tmpX;
	deltaRgbY = 255 / tmpY;
	deltaRgbZ = 255 / tmpZ;

	for (int j = 0; j < width; j++)
	{
		r = 255 * (cloud->points[j].z - minValueZ) / tmpZ;
		g = 255 * (cloud->points[j].z - minValueZ) / tmpZ;
		b = 255 * (cloud->points[j].z - minValueZ) / tmpZ;

		cloud->points[j].r = r;
		cloud->points[j].g = g;
		cloud->points[j].b = b;
	}
	return 0;
}

double pclCloud::myMax(double x, double y, double z)
{
	double maxValue, maxTmp1, maxTmp2;
	double x1, y1, z1;
	x1 = abs(x);
	y1 = abs(y);
	z1 = abs(z);

	maxTmp1 = (x1 > y1 ? x1 : y1);
	maxTmp2 = (x1 > z1 ? x1 : z1);
	maxValue = (maxTmp1 > maxTmp2 ? maxTmp1 : maxTmp2);

	return maxValue;
}

void pclCloud::enable()
{
	optDisable = true;
}

void pclCloud::disable()
{
	optDisable = false;
}

bool pclCloud::isEnable()
{
	return optDisable;
}
